int GUARD_GAIN = 180, Setpoint = 0;
double kP = 1.0 , kI = 0.0 , kD = 0.02;
double error, last_error, pTerm, iTerm, dTerm, integrated_error;
double K = 1;

int updatePid(int currentPosition)   {
  error = Setpoint - currentPosition; 
  pTerm = kP * error;
  integrated_error += error;                                       
  iTerm = constrain( kI * integrated_error, -GUARD_GAIN, GUARD_GAIN);
  dTerm = kD * (error - last_error);                            
  last_error = error;
  return -constrain(K*(pTerm + iTerm + dTerm), -GUARD_GAIN, GUARD_GAIN);
}
